/*
 * @Description: 数据预处理模块，包括时间同步、点云去畸变等
 * @Author: Zhijian Qiao
 * @Date: 2020-02-10 08:31:22
 */
#ifndef avp_slam_DATA_PRETREAT_DATA_PRETREAT_FLOW_HPP_
#define avp_slam_DATA_PRETREAT_DATA_PRETREAT_FLOW_HPP_

#include <ros/ros.h>
// subscriber
#include "subscriber/imu_subscriber.hpp"
#include "subscriber/velocity_subscriber.hpp"
#include "subscriber/four_images_subscriber.hpp"
#include "tf_listener/tf_listener.hpp"
#include "models/IPM/IPManager.h"
// publisher
#include "publisher/odometry_publisher.hpp"
#include "publisher/tf_broadcaster.hpp"
#include "publisher/image_publisher.hpp"
// models
#include <yaml-cpp/yaml.h>
#include "tools/tic_toc.hpp"
#include "ahrs/Estimator.h"
#include "sensor_data/pose_data.hpp"
#include "sensor_data/image_data.hpp"


namespace avp_slam {
    class DataPretreatFlow {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        DataPretreatFlow(ros::NodeHandle &nh, std::string images_topic);

        bool Run();

    private:
        bool ReadData();

        bool HasData();

        bool updatePred();

        bool ValidData();

        bool Preintegration();

        Eigen::Vector3d updateU(std::deque<VelocityData> &unsynced_velocity, double front_time, double back_time);

        bool TransformSensorData();

        Eigen::Matrix4d Fusing_IMU_VelOdom(IMUData imu_front, IMUData imu_back);

        bool PublishData();

    private:

        std::shared_ptr<OriEst::Estimator> orientation_estimator;

        // subscriber
        std::shared_ptr<IMUSubscriber> dr_sub_ptr_;
        std::shared_ptr<VelocitySubscriber> velocity_sub_ptr_;
        std::shared_ptr<FourImagesSubscriber> images_sub_ptr_;

        // publisher
        std::shared_ptr<OdometryPublisher> imu_pub_ptr_;
        std::shared_ptr<OdometryPublisher> images_pred_pub_ptr;
        std::shared_ptr<ImagePublisher> image_publisher_ptr;
        std::shared_ptr<ImagePublisher> concat_image_publisher_ptr;

        std::deque<FourImagesData> images_data_buff_;

        std::deque<IMUData> raw_imu_;
        std::deque<IMUData> unsynced_imu_;
        std::deque<PoseData> unsynced_odo_;
        std::deque<VelocityData> unsynced_velocity_;

        IMUData current_imu_data_;
        VelocityData current_velocity_data_;
        ImageData current_images_data_;
        ImageData previous_images_data_;
        ImageData concat_images_data_;


        std::deque<PoseData> odo_buff_;

        Eigen::Matrix4f T_pred;

        //status
        bool has_previous_images = false;
        bool has_T_pred = false;
        //config
        YAML::Node config_node_;

        std::shared_ptr<IPManager> ipm_manager;

        std::shared_ptr<TicToc> time_sync;
        std::shared_ptr<TicToc> time_pred;
        std::shared_ptr<TicToc> time_publish;
        bool adjust_valid;
    };
}

#endif